#include <stdio.h>
#include <windows.h>
#include "pThreads/semaphore.h"
#include <vector>
#include "CSemaphore.h"
#include <math.h>
#include "CBasicPacket.h"
#include "CConsole.h"

CBasicPacket::CBasicPacket() {
	m_packetIdentifier = 0;
	m_dontProcessOnZone = 0;
}

CBasicPacket::~CBasicPacket() {
}

unsigned char CBasicPacket::getDontProcessOnZoneValue(void)
{
	return this->m_dontProcessOnZone;
}

long long CBasicPacket::getPacketIdentifier(void)
{
	return this->m_packetIdentifier;
}

// return the size of the packet
unsigned char CBasicPacket::getSize() {
	return this->size;
}

unsigned char CBasicPacket::getType() {
	return this->type;
}

// sets the count value for the packet
void CBasicPacket::setNum(unsigned short num) {
	this->num = num;
}

void CBasicPacket::write(unsigned char * buf) {
	memset(buf, 0, this->getSize()*2);
	memcpy(buf, this, this->getSize()*2);
}

void CBasicPacket::packBitsBE(unsigned char* target, unsigned long long value, int bitOffset, unsigned char lengthInBit)
{
	CBasicPacket::packBitsBE(target, value, 0, bitOffset, lengthInBit);
}

void CBasicPacket::packBitsBE(unsigned char* target, unsigned long long value, int byteOffset, int bitOffset, unsigned char lengthInBit)
{
	//correct bitOffsets>=8
	byteOffset+=(int)(bitOffset/8);
	bitOffset%=8;

	//Generate bitmask
	unsigned long long bitmask;
	bitmask=0xFFFFFFFFFFFFFFFFLL;
	bitmask>>=(64-lengthInBit);
	bitmask<<=bitOffset;
	
	//shift value
	value<<=bitOffset;
	value&=bitmask;

	//invert bitmask
	bitmask^=0xFFFFFFFFFFFFFFFFLL;

	//write shifted value to target
	if ((lengthInBit+bitOffset)<=8)
	{
		unsigned char bitmaskUC;
		unsigned char valueUC;
		unsigned char* dataPointer;
		dataPointer=(unsigned char*)&target[byteOffset];

		bitmaskUC=(unsigned char)bitmask&0xFF;
		valueUC=(unsigned char)value;

		*dataPointer&=bitmaskUC;
		*dataPointer|=valueUC;
	}
	else if ((lengthInBit+bitOffset)<=16)
	{
		unsigned short bitmaskUS;
		unsigned short valueUS;
		unsigned short* dataPointer;
		dataPointer=(unsigned short*)&target[byteOffset];

		bitmaskUS=(unsigned short)bitmask&0xFFFF;
		valueUS=(unsigned short)value;

		*dataPointer&=bitmaskUS;
		*dataPointer|=valueUS;
	}
	else if ((lengthInBit+bitOffset)<=32)
	{
		unsigned int bitmaskUI;
		unsigned int valueUI;
		unsigned int* dataPointer;
		dataPointer=(unsigned int*)&target[byteOffset];

		bitmaskUI=(unsigned int)bitmask&0xFFFFFFFF;
		valueUI=(unsigned int)value;

		*dataPointer&=bitmaskUI;
		*dataPointer|=valueUI;
	}
	else if ((lengthInBit+bitOffset)<=64)
	{
		unsigned long long* dataPointer;
		dataPointer=(unsigned long long*)&target[byteOffset];

		*dataPointer&=bitmask;
		*dataPointer|=value;
	}
	else
	{
		CConsole::outErr("Pack Bits Error: packBits(...) not implemented for targetsizes above 64 bits.\n Targetsize: %d",(lengthInBit+bitOffset));
	}
}

unsigned long long CBasicPacket::unpackBitsBE(unsigned char* target,int bitOffset, unsigned char lengthInBit)
{
	return CBasicPacket::unpackBitsBE(target,0,bitOffset,lengthInBit);
}

unsigned long long CBasicPacket::unpackBitsBE(unsigned char* target,int byteOffset, int bitOffset, unsigned char lengthInBit)
{
	//correct bitOffsets>=8
	byteOffset+=(int)(bitOffset/8);
	bitOffset%=8;

	//generate bitmask
	unsigned long long bitmask;
	bitmask=0xFFFFFFFFFFFFFFFFLL;
	bitmask>>=(64-lengthInBit);
	bitmask<<=bitOffset;

	unsigned long long retVal;

	//get Value
	if ((lengthInBit+bitOffset)<=8)
	{
		unsigned char bitmaskUC;
		unsigned char valueUC;
		unsigned char* dataPointer;
		dataPointer=(unsigned char*)&target[byteOffset];

		bitmaskUC=(unsigned char)bitmask&0xFF;
		valueUC=(*dataPointer)&bitmaskUC;
		valueUC>>=bitOffset;

		retVal=(unsigned long long)valueUC;
	}
	else if ((lengthInBit+bitOffset)<=16)
	{
		unsigned short bitmaskUS;
		unsigned short valueUS;
		unsigned short* dataPointer;
		dataPointer=(unsigned short*)&target[byteOffset];

		bitmaskUS=(unsigned short)bitmask&0xFFFF;
		valueUS=(*dataPointer)&bitmaskUS;
		valueUS>>=bitOffset;

		retVal=(unsigned long long)valueUS;
	}
	else if ((lengthInBit+bitOffset)<=32)
	{
		unsigned int bitmaskUI;
		unsigned int valueUI;
		unsigned int* dataPointer;
		dataPointer=(unsigned int*)&target[byteOffset];

		bitmaskUI=(unsigned int)bitmask&0xFFFFFFFF;
		valueUI=(*dataPointer)&bitmaskUI;
		valueUI>>=bitOffset;

		retVal=(unsigned long long)valueUI;
	}
	else if ((lengthInBit+bitOffset)<=64)
	{
		unsigned long long valueULL;
		unsigned long long* dataPointer;
		dataPointer=(unsigned long long*)&target[byteOffset];

		valueULL=(*dataPointer)&bitmask;
		valueULL>>=bitOffset;

		retVal=(unsigned long long)valueULL;
	}
	else
	{
		CConsole::outErr("Unpack Bits Error: unpackBits(...) not implemented for targetsizes above 64 bits.\n Targetsize: %d",(lengthInBit+bitOffset));
		return 0;
	}
	return retVal;
}

//Little Endian (un)pack functions

unsigned long long CBasicPacket::unpackBitsLE(char* target,int bitOffset, unsigned char lengthInBit)
{
	return CBasicPacket::unpackBitsLE((unsigned char*)target,0,bitOffset,lengthInBit);
}

unsigned long long CBasicPacket::unpackBitsLE(unsigned char* target,int bitOffset, unsigned char lengthInBit)
{
	return CBasicPacket::unpackBitsLE(target,0,bitOffset,lengthInBit);
}

unsigned long long CBasicPacket::unpackBitsLE(unsigned char* target,int byteOffset, int bitOffset, unsigned char lengthInBit)
{
	//correct bitOffsets>=8
	byteOffset+=(int)(bitOffset/8);
	bitOffset%=8;

	unsigned long long retVal;
	unsigned char* modifiedTarget;

	//calculate how many bytes are needed
	unsigned char bytesNeeded;
	if ((bitOffset + lengthInBit)<=8)
		bytesNeeded=1;
	else if ((bitOffset + lengthInBit)<=16)
		bytesNeeded=2;
	else if ((bitOffset + lengthInBit)<=32)
		bytesNeeded=4;
	else if ((bitOffset + lengthInBit)<=64)
		bytesNeeded=8;
	else
	{
		CConsole::outErr("Unpack Bits Error: unpackBits(...) not implemented for targetsizes above 64 bits.\n Targetsize: %d",(lengthInBit+bitOffset));
		return 0;
	}

	//convert byteOrder to Big Endian
	modifiedTarget=new unsigned char[bytesNeeded];

	for (unsigned char curByte=0;curByte<bytesNeeded;curByte++)
	{
		modifiedTarget[curByte]=target[byteOffset+(bytesNeeded-1)-curByte];
	}

	//unpack Bits
	if (bytesNeeded==1)
	{
		unsigned char bitmask = 0xFF >> bitOffset;

		retVal = (modifiedTarget[0]&bitmask) >> (8-(lengthInBit+bitOffset));
	}
	else
	{
		//calculate new bitOffset
		int newBitOffset=(bytesNeeded*8)-(bitOffset+lengthInBit);

		retVal = unpackBitsBE(&modifiedTarget[0],0,newBitOffset,lengthInBit);
	}

	return retVal;
}

void CBasicPacket::packBitsLE(char* target, unsigned long long value, int bitOffset, unsigned char lengthInBit)
{
	CBasicPacket::packBitsLE((unsigned char*)target, value, 0, bitOffset, lengthInBit);
}

void CBasicPacket::packBitsLE(unsigned char* target, unsigned long long value, int bitOffset, unsigned char lengthInBit)
{
	CBasicPacket::packBitsLE(target, value, 0, bitOffset, lengthInBit);
}

void CBasicPacket::packBitsLE(unsigned char* target, unsigned long long value, int byteOffset, int bitOffset, unsigned char lengthInBit)
{
	//correct bitOffsets>=8
	byteOffset+=(int)(bitOffset/8);
	bitOffset%=8;

	unsigned char* modifiedTarget;

	//calculate how many bytes are needed
	unsigned char bytesNeeded;
	if ((bitOffset + lengthInBit)<=8)
		bytesNeeded=1;
	else if ((bitOffset + lengthInBit)<=16)
		bytesNeeded=2;
	else if ((bitOffset + lengthInBit)<=32)
		bytesNeeded=4;
	else if ((bitOffset + lengthInBit)<=64)
		bytesNeeded=8;
	else
	{
		CConsole::outErr("Pack Bits Error: packBitsLE(...) not implemented for targetsizes above 64 bits.\n Targetsize: %d",(lengthInBit+bitOffset));
		return ;
	}

	//convert byteOrder to Big Endian
	modifiedTarget=new unsigned char[bytesNeeded];

	for (unsigned char curByte=0;curByte<bytesNeeded;curByte++)
	{
		modifiedTarget[curByte]=target[byteOffset+(bytesNeeded-1)-curByte];
	}

	//calculate new bitOffset
	int newBitOffset=(bytesNeeded*8)-(bitOffset+lengthInBit);

	//write data to modified array
	packBitsBE(&modifiedTarget[0],value,0,newBitOffset,lengthInBit);

	//copy back to target
	for (unsigned char curByte=0;curByte<bytesNeeded;curByte++)
	{
		target[byteOffset+(bytesNeeded-1)-curByte]=modifiedTarget[curByte];
	}
}